
package simulator;
import java.awt.*;
import java.util.*;
/**
 * Models the behavior and output of the iRobot Create's builtin 
 * IR beacon sensor, allowing for home base location.
 * REDBUOY is the signal emitted when the base station is on the left.
 * GREENBUOY is the signal emitted when the base station is on the right.
 * FORCEFIELD is the signal emitted when the Robot is particularly close to 
 * the homebase
 */
public class BeaconSensor extends Sensor{
    public static final int REDBUOY = 0;
    public static final int GREENBUOY = 1;
    public static final int FORCEFIELD = 2;
    public static final double ABSENT = 0.0;
    public static final double PRESENT = 1.0;
    public static final int RANGE = 150;
    public static final int FORCERANGE = 60;
    
  public BeaconSensor(Environment e, double x, double y){
    super(e);
    this.x=x;
    this.y=y;
  }
 
  public double[] read(int magicnum) {  
    double[] reading = new double[4];
	reading[GREENBUOY] = ABSENT;
	reading[REDBUOY] = ABSENT;
	reading[FORCEFIELD] = ABSENT;
    ArrayList<PhysicalObject> known=env.getObjects();
	
	for(PhysicalObject o : known){
           if(o.getClass().getName().equals(Beacon.class.getName())){
           //so we have a beacon, and now we get the distance from the beacon to us
             if(distance(o.getX(), o.getY(), this.x, this.y) <= RANGE){
           //now we want to figure out if it's in green, red , both
            	 reading[GREENBUOY] = PRESENT;
            	 reading[REDBUOY] = PRESENT;
            	 //TODO fix beaconsensor
             }
             if(distance(o.getX(), o.getY(), this.x, this.y) <= FORCERANGE){
                 //now we want to figure out if it's in green, red , both
                  	 reading[FORCEFIELD] = PRESENT;
                  	 //TODO Simulate BeaconSensor directionality
                   }
             
           }
        }
	reading[3] = Simulator.time;
	setState(reading);
	return reading;
  }
  public int getID() {
      return 3;
  }

  public void paint(Graphics g) {
    g.setColor(Color.RED);
    
    g.setColor(new Color(0.f, 1.f, 0.f, .75f));
    g.fillArc((int)(env.getRobotX() +x) - RANGE / 2,
              (int)(env.getRobotY() +y) - RANGE / 2,
              RANGE,
              RANGE,
              -20 + -1 * (int)((env.getRobotAngle() * 360 / (Math.PI * 2)) % 360),
              10);

    g.setColor(new Color(1.f, 1.f, 0.f, .75f));
    g.fillArc((int)(env.getRobotX() +x) - RANGE / 2,
              (int)(env.getRobotY() +y) - RANGE / 2,
              RANGE,
              RANGE,
              -10 + -1 * (int)((env.getRobotAngle() * 360 / (Math.PI * 2)) % 360),
              20);

    g.setColor(new Color(1.f, 0.f, 0.f, .75f));
    g.fillArc((int)(env.getRobotX() +x) - RANGE / 2,
              (int)(env.getRobotY() +y) - RANGE / 2,
              RANGE,
              RANGE,
              10 + -1 * (int)((env.getRobotAngle() * 360 / (Math.PI * 2)) % 360),
              10);

  }
  
  public String getName(){
    return "BeaconSensor";  //add id as well?
  }
  
  public static double distance(double x1, double y1, double x2, double y2){
    return Math.sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
  }
}